#include "stm32f4xx.h"
#include "bsp.h"

static u8 UART_RX_BUF[26];
u16 SBUS_ch_value[16];

/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void Sbus_Data_Count(u16 *out_arry, u8 *in_arry)
{
		*(out_arry + 0)= (( *(in_arry + 2)&0x07) << 8 ) + *(in_arry + 1); //sBus[ 2] low3 + sBus[ 1] low8
		*(out_arry + 1) = ( ( * (in_arry + 3)&0x3F) << 5 ) + ( * (in_arry + 2) >> 3 ); //sBus[ 3] low6 + sBus[ 2] high5
		*(out_arry + 2) = ( ( * (in_arry + 5)&0x01) << 10) + ( * (in_arry + 4) << 2 ) + ( * (in_arry + 3) >> 6); //sBus[ 5] low1 + sBus[ 4] low8 + sBus[ 3] high2
		*(out_arry + 3) = ( ( * (in_arry + 6)&0x0F) << 7 ) + ( * (in_arry + 5) >> 1 ); //sBus[ 6] low4 + sBus[ 5] high7
		*(out_arry + 4) = ( ( * (in_arry + 7)&0x7F) << 4 ) + ( * (in_arry + 6) >> 4 ); //sBus[ 7] low7 + sBus[ 6] high4
		*(out_arry + 5) = ( ( * (in_arry + 9)&0x03) << 9 ) + ( * (in_arry + 8) << 1 ) + ( * (in_arry + 7) >> 7); //sBus[ 9] low2 + sBus[ 8] low8 + sBus[ 7] high1
		*(out_arry + 6) = ( ( * (in_arry +10)&0x1F) << 6 ) + ( * (in_arry + 9) >> 2 ); //sBus[10] low5 + sBus[ 9] high6
		*(out_arry + 7) = ( ( * (in_arry +11)&0xFF) << 3 ) + ( * (in_arry +10) >> 5 ); //sBus[11] low8 + sBus[10] high3

		*(out_arry + 8) = ( ( * (in_arry +13)&0x07) << 8 ) + ( * (in_arry + 12)); //sBus[13] low3 + sBus[12] low8
		*(out_arry + 9) = ( ( * (in_arry +14)&0x3F) << 5 ) + ( * (in_arry +13) >> 3 ); //sBus[14] low6 + sBus[13] high5
		*(out_arry + 10) = ( ( * (in_arry +16)&0x01) << 10) + ( * (in_arry +15) << 2 ) + ( * (in_arry +14) >> 6); //sBus[16] low1 + sBus[15] low8 + sBus[14] high2
		*(out_arry + 11) = ( ( * (in_arry +17)&0x0F) << 7 ) + ( * (in_arry +16) >> 1 ); //sBus[17] low4 + sBus[16] high7
		*(out_arry + 12) = ( ( * (in_arry +18)&0x7F) << 4 ) + ( * (in_arry +17) >> 4 ); //sBus[18] low7 + sBus[17] high4
		*(out_arry + 13) = ( ( * (in_arry +20)&0x03) << 9 ) + ( * (in_arry +19) << 1 ) + ( * (in_arry +18) >> 7); //sBus[20] low2 + sBus[19] low8 + sBus[18] high1
		*(out_arry + 14) = ( ( * (in_arry +21)&0x1F) << 6 ) + ( * (in_arry +20) >> 2 ); //sBus[21] low5 + sBus[20] high6
		*(out_arry + 15) = ( ( * (in_arry +22)&0xFF) << 3 ) + ( * (in_arry +21) >> 5 ); //sBus[22] low8 + sBus[21] high3

}
/**
  * @name
  * @brief
  * @param  
  * @retval 
  */
void SBUS_RxHandler(void)
{
		u8 res;
		u8 clear = 0;
		static u8 Rx_Sta = 1;
		//sigle byte receive finish
		if(USART_GetITStatus(SBUS_UARTx, USART_IT_RXNE) != RESET)
		{
			res = SBUS_UARTx->DR;
			UART_RX_BUF[Rx_Sta++] = res;
		}
		//sbus frame receive finish
		else if(USART_GetITStatus(SBUS_UARTx, USART_IT_IDLE) != RESET)
		{
			clear = SBUS_UARTx->SR;
			clear = SBUS_UARTx->DR;
			UART_RX_BUF[0] = Rx_Sta - 1;
			Rx_Sta = 1;
			
			if(UART_RX_BUF[1] == 0x0F)//check header of data frame is correct or not(0x0F)
				Sbus_Data_Count(SBUS_ch_value ,UART_RX_BUF + 1);
		}
}



/******************************END OF FILE SBUS.C*************************************/
